extends Node3D

@export var battery_scene: PackedScene

var batter_on_cabinet = false

enum Step {
	STEP_NONE = 0,
	STEP_LIFT = 1,
	STEP_SLIDE_OUT = 2,
	STEP_SLIDE = 3,
	STEP_TROP = 4
}

enum State {
	STATE_IDLE = 0,
	STATE_BUSY = 1,
	STATE_STOPPED = 2,
}

enum ExchangeStep {
	EXCHANGE_NONE = 0,
	EXCHANGE_VEH_TO_TRANSIT = 1,
	EXCHANGE_CAB_TO_VEH = 2,
	EXCHANGE_TRANSIT_TO_CAB = 3,
	EXCHANGE_STOPPED = 4
}

enum Cmd {
	CMD_NONE = 0,
	CMD_START = 1,
	CMD_STOP = 2,
}

var step = Step.STEP_NONE
var transit_cab_no = 7
var cabinets = []
var online_veh = null
var cmd = Cmd.CMD_NONE
var state = State.STATE_IDLE
var exchange_step = ExchangeStep.EXCHANGE_NONE
var carrying_bat = null
var from_cab = null
var to_cab = null
var cabinet_no = 0
var chg = load("res://charger.gd").new()

func _ready():
	var tmp_cabinets = [$Shelf/Cabinet7, $Shelf/Cabinet6, $Shelf/Cabinet5, $Shelf/Cabinet4, $Shelf/Cabinet3, $Shelf/Cabinet2, $Shelf/Cabinet1, $Shelf/Cabinet8]
	var tmp_batteries = [$Battery, $Battery2, $Battery3, $Battery4, $Battery5, $Battery6, null, null]
	cabinets = tmp_cabinets
	cabinets[transit_cab_no].set_auto_charge(false)
	online_veh = $Vehicle
	for bat in tmp_batteries:
		if (bat) :
			bat._set_soc(80)
	print($Vehicle.get_cabinet_pos())
	print($Battery.position)
	step = Step.STEP_LIFT
	cmd = Cmd.CMD_START
	from_cab = cabinets[0]
	to_cab = online_veh.get_cabinet()
	
	print("add a  battery")
	var bat = battery_scene.instantiate()
	var pos = online_veh.get_cabinet_pos()
	pos.y = pos.y + 0.5
	bat._set_pos(pos)
	bat._set_soc(80)
	online_veh.set_battery(bat)
	add_child(bat)

func cab_switch_battery(from, to):
	#var bat = from.get_battery()
	var bat_vel = Vector3.ZERO
	var bat_pos = Vector3.ZERO
	var to_cab_pos = to.position
	var transit_cab_pos = cabinets[transit_cab_no].position + $Shelf.position

	if (state == State.STATE_STOPPED):
		state = State.STATE_IDLE
		return

	if (cmd == Cmd.CMD_START):
		carrying_bat = from.get_battery()
		state = State.STATE_BUSY
		step = Step.STEP_LIFT
		cmd = Cmd.CMD_NONE
		print("switch start")

	if (state == State.STATE_IDLE):
		return

	if (!carrying_bat):
		return
	bat_pos = carrying_bat.position

	if (to == $Vehicle.get_cabinet()):
		to_cab_pos = $Vehicle.get_cabinet_pos()
	else :
		to_cab_pos = to_cab_pos + $Shelf.position

	if (abs(bat_pos.x - to_cab_pos.x) < 0.05):
		bat_pos.x = to_cab_pos.x
		carrying_bat._set_pos(bat_pos)
		if (abs(bat_pos.z - to_cab_pos.z) < 0.05):
			bat_pos.z = to_cab_pos.z
			carrying_bat._set_pos(bat_pos)
			if (step == Step.STEP_SLIDE):
				step = Step.STEP_TROP
				print("to trop")

	match step:
		Step.STEP_LIFT:
			if (bat_pos.y < 3):
				bat_vel.y = 0.5
			else :
				step = Step.STEP_SLIDE_OUT
				print("to slide out")
		Step.STEP_SLIDE_OUT:
			#var transit_cab_pos = cabinets[transit_cab_no].position + $Shelf.position
			#print(transit_cab_pos)
			if (abs(bat_pos.z - transit_cab_pos.z) < 0.05):
				bat_pos.z = transit_cab_pos.z
				carrying_bat._set_pos(bat_pos)
				step = Step.STEP_SLIDE
				print("to slide")
			else:
				if (bat_pos.z < transit_cab_pos.z) :
					bat_vel.z = 0.5
				if (bat_pos.z > transit_cab_pos.z) :
					bat_vel.z = -0.5
			#step = Step.STEP_SLIDE
		Step.STEP_SLIDE:
			if (bat_pos.x < to_cab_pos.x) :
				bat_vel.x = 0.5
			if (bat_pos.x > to_cab_pos.x) :
				bat_vel.x = -0.5
			if (bat_vel.y == 0 && bat_vel.x == 0) :
				if (bat_pos.z < to_cab_pos.z) :
					bat_vel.z = 0.5
				if (bat_pos.z > to_cab_pos.z) :
					bat_vel.z = -0.5
		Step.STEP_TROP:
			if (!batter_on_cabinet) :
				bat_vel.y = -0.5
		
	carrying_bat._set_velocity(bat_vel)

func _physics_process(delta):
	#place_on_veh($Battery)
	cab_switch_battery(from_cab, to_cab)

func _start_shift(from, to):
	if (cmd == Cmd.CMD_NONE):
		batter_on_cabinet = false
		cmd = Cmd.CMD_START
		from_cab = from
		to_cab = to

func _battery_on_cab(cab):
	batter_on_cabinet = true
	step = Step.STEP_NONE
	carrying_bat = null
	state == State.STATE_STOPPED
	match exchange_step:
		ExchangeStep.EXCHANGE_VEH_TO_TRANSIT:
			exchange_step = ExchangeStep.EXCHANGE_CAB_TO_VEH
			_start_shift(cabinets[cabinet_no], online_veh.get_cabinet())
			print("exchange to cab_to_veh")
		ExchangeStep.EXCHANGE_CAB_TO_VEH:
			exchange_step = ExchangeStep.EXCHANGE_TRANSIT_TO_CAB
			_start_shift(cabinets[transit_cab_no], cabinets[cabinet_no])
			print("exchange to transit_to_cab")
		ExchangeStep.EXCHANGE_TRANSIT_TO_CAB:
			cabinet_no = (cabinet_no + 1) % 7
			exchange_step = ExchangeStep.EXCHANGE_NONE

func _on_vehicle_bat_placed():
	print("battery placed on veh")
	if (online_veh):
		_battery_on_cab(online_veh.get_cabinet())

func _on_button_pressed():
	chg.chg_connect()
	if (exchange_step == ExchangeStep.EXCHANGE_NONE):
		exchange_step = ExchangeStep.EXCHANGE_VEH_TO_TRANSIT
		_start_shift(online_veh.get_cabinet(), cabinets[transit_cab_no])


func _on_cabinet_bat_detect(bat):
	print("battery placed on cabine")
	_battery_on_cab($Shelf/Cabinet)

func _on_cabinet_2_bat_detect(bat):
	print("battery placed on cabine")
	_battery_on_cab($Shelf/Cabinet2)


func _on_cabinet_3_bat_detect(bat):
	print("battery placed on cabine")
	_battery_on_cab($Shelf/Cabinet3)


func _on_cabinet_4_bat_detect(bat):
	print("battery placed on cabine")
	_battery_on_cab($Shelf/Cabinet4)


func _on_cabinet_5_bat_detect(bat):
	print("battery placed on cabine")
	_battery_on_cab($Shelf/Cabinet5)


func _on_cabinet_6_bat_detect(bat):
	print("battery placed on cabine")
	_battery_on_cab($Shelf/Cabinet6)


func _on_cabinet_7_bat_detect(bat):
	print("battery placed on cabine")
	_battery_on_cab($Shelf/Cabinet7)


func _on_cabinet_8_bat_detect(bat):
	print("battery placed on cabine")
	_battery_on_cab($Shelf/Cabinet8)
